import json
import time
import numpy as np
import math
import sys
import os
import pickle
from typing import List, Dict, Tuple, Optional
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.collections import PatchCollection
from matplotlib.patches import Rectangle
from itertools import groupby  # 用于任务分组

from area_coverage_planner import AreaCoveragePlanner
from coord_transform import PositionConvert
# from SpatialAstar3D import SpaceTimeAStar3D, multi_uav_planning_3d, load_map_data, ReservationTable, SLACK
from SpatialAstar3DExit import SpaceTimeAStar3D, multi_uav_planning_3d, load_map_data, ReservationTable, SLACK
# 动画配置参数
TRAIL_LENGTH = 10  # 轨迹尾迹包含的历史点数量

class MultiUAVPlanner:
    """多无人机任务规划与路径生成库（含3D可视化）"""
    def __init__(self, map_env, ref_lat, ref_lon,
                # 区域覆盖规划器参数
                patrol_speed=10.0,
                min_patrol_speed=5.0,
                max_patrol_speed=25.0,
                target_patrol_time=600.0,
                time_buffer=120.0,
                default_altitude=10.0,
                sweep_width=12.0,
                max_segment_length=20.0,
                path_boundary_standoff=5.0,
                # 3D规划器参数
                speed=20.0,
                safe_dis=2.0,
                min_planning_speed=5.0,
                max_planning_speed=25.0,
                speed_increase_factor=1.1,
                max_t=3000,
                goal_tolerance=2.0,
                heuristic_weight=1.5,              
                ):
        
        """初始化多无人机规划器，支持外部配置核心参数
        
        参数说明：
        ----------
        区域覆盖规划器参数（用于巡航、区域扫查任务）：
            patrol_speed : float, 默认10.0
                巡航任务初始速度（直线段飞行速度），单位：米/秒（m/s）
            min_patrol_speed : float, 默认5.0
                巡航速度最小值（速度自适应调整时的下限），单位：m/s，需小于max_patrol_speed
            max_patrol_speed : float, 默认25.0
                巡航速度最大值（速度自适应调整时的上限），单位：m/s，需大于min_patrol_speed
            time_buffer : float, 默认120.0
                任务时间缓冲量（预留时间，避免任务超时），单位：秒（s）
            target_patrol_time : float, 默认600.0
                巡航任务时间约束（任务必须在此时间内完成），单位：秒（s）
            default_altitude : float, 默认10.0
                无指定高度时的默认飞行高度，单位：米（m）
            sweep_width : float, 默认12.0
                传感器扫描宽度（决定巡航路径间隔），单位：m，影响区域覆盖密度
            max_segment_length : float, 默认20.0
                巡航路径段的最大长度（超过则自动拆分），单位：m
            path_boundary_standoff : float, 默认5.0
                巡航路径与区域边界的安全距离（避免贴近边界飞行），单位：m
        
        3D规划器参数（用于路径搜索、避障、时间约束控制）：
            speed : float, 默认20.0
                3D路径规划初始速度（无人机飞行基础速度），单位：m/s
            safe_dis : float, 默认2.0
                无人机与障碍物的安全距离（避障最小间距），单位：m，值越大避障越保守
            min_planning_speed : float, 默认5.0
                规划速度最小值（路径搜索时的速度下限），单位：m/s，需小于max_planning_speed
            max_planning_speed : float, 默认25.0
                规划速度最大值（路径搜索时的速度上限），单位：m/s，需大于min_planning_speed
            speed_increase_factor : float, 默认1.1
                速度调整系数（规划失败时，速度按此系数递增尝试重新规划），需大于1
            max_t : int, 默认3000
                最大规划时间（路径总耗时超过此值则判定为规划失败），单位：s
            goal_tolerance : float, 默认2.0
                目标点位置容差（允许的最终位置误差），单位：m
            heuristic_weight : float, 默认1.5
                A*算法启发式权重（平衡搜索速度与路径最优性），推荐范围1.1~2.0，值越大搜索越快
        
        必传基础参数：
            map_env : 
                地图环境数据（包含障碍物信息），用于3D路径避障
            ref_lat : float
                地图参考点纬度（用于GPS与局部坐标转换），单位：度
            ref_lon : float
                地图参考点经度（用于GPS与局部坐标转换），单位：度
        """
        self.pos_converter = PositionConvert()  # 经纬度与平面坐标转换器

        # 基础参数（外部可配置）
        self.min_patrol_speed = min_patrol_speed
        self.max_patrol_speed = max_patrol_speed
        self.time_buffer = time_buffer
        self.default_altitude = default_altitude
        self.sweep_width = sweep_width
        self.max_segment_length = max_segment_length

        # 3D规划器参数
        self.speed = speed  # 初始规划速度
        self.safe_dis = safe_dis  # 无人机与障碍物的安全距离
        self.min_planning_speed = min_planning_speed
        self.max_planning_speed = max_planning_speed
        self.speed_increase_factor = speed_increase_factor
        self.max_t = max_t
        self.goal_tolerance = goal_tolerance
        self.map_env = map_env
        self.ref_lat = ref_lat  # 地图参考点纬度
        self.ref_lon = ref_lon  # 地图参考点经度
        self.heuristic_weight = heuristic_weight  # 启发式权重
        #区域覆盖器参数
        self.scan_speed=patrol_speed  # 初始巡航速度
        self.target_patrol_time=target_patrol_time#巡航任务时间约束
        self.boundary_standoff=path_boundary_standoff#与巡航任务边界距离
        # 存储数据
        self.uav_positions = {}
        self.uav_goals = {}
        self.task_time_constraints = {}
        self.cruise_data = {}
        self.all_time_paths = {}  # 最终栅格时空路径（用于预留表）
        self.patrol_entry_points = {}
        self.animation_timestamps = []
        self.final_planning_speeds = {}
        self.final_patrol_speeds = {}
        # 存储每个无人机对应的规划器（用于可视化）
        self.uav_planner = {}
        # per-UAV 限制（由外部任务字段填充）
        # key: uav_id -> float
        self.uav_max_planning_speed = {}
        self.uav_max_patrol_speed = {}
        # 将 reservation_table 初始化为类成员
        self.reservation_table = ReservationTable()
    def _reset_internal_state(self):
        """重置所有内部状态以便处理新的任务集"""
        self.uav_positions = {}
        self.uav_goals = {}
        self.task_time_constraints = {}
        self.cruise_data = {}
        self.all_time_paths = {}
        self.patrol_entry_points = {}
        self.animation_timestamps = []
        self.final_planning_speeds = {}
        self.final_patrol_speeds = {}
        self.uav_planner = {}  # 重置规划器存储
        self.uav_max_planning_speed = {}
        self.uav_max_patrol_speed = {}
        # 同时重置预留表
        self.reservation_table = ReservationTable()

    def process_tasks(self, tasks_json: List[Dict]) -> Tuple[Dict[int, List[Dict]], Dict[int, List[Tuple[float, float, float, float]]]]:
        """
        处理任务列表，按优先级分组进行统一规划，支持多段途径点规划
        """
        self._reset_internal_state()

        # 预填充 per-UAV 速度限制（来自任务字段）
        for task in tasks_json:
            for uid in task.get('uav_ids', []):
                # 规划速度优先级：任务级 max_planning_speed -> planner 默认 self.max_planning_speed
                if task.get('max_planning_speed') is not None:
                    try:
                        self.uav_max_planning_speed[uid] = float(task.get('max_planning_speed'))
                    except Exception:
                        pass
                # 巡航速度限制
                if task.get('max_patrol_speed') is not None:
                    try:
                        self.uav_max_patrol_speed[uid] = float(task.get('max_patrol_speed'))
                        self.uav_max_planning_speed[uid] = float(task.get('max_patrol_speed'))
                    except Exception:
                        pass

        # 检查是否有需要多段规划的任务
        multi_waypoint_tasks = [task for task in tasks_json if task.get('taskType') == 'multi_waypoint']
        if multi_waypoint_tasks:
            return self._process_multi_waypoint_tasks(tasks_json)
        else:
            return self._process_single_segment_tasks(tasks_json)

    def _process_single_segment_tasks(self, tasks_json: List[Dict]):
        """处理单段任务"""
        sorted_tasks = sorted(tasks_json, key=lambda x: x['priority'])

        # 先解析所有无人机起点和时间约束
        for task in sorted_tasks:
            self._process_start_positions(task['start'])
            for uav_id in task['uav_ids']:
                self.task_time_constraints[uav_id] = task['time_constraint']

        all_uav_trajectories = {}
        final_world_paths = {}

        # 按优先级分组规划
        for priority, tasks_in_group in groupby(sorted_tasks, key=lambda x: x['priority']):
            group_tasks = list(tasks_in_group)
            uavs_in_group = list(set(uav_id for task in group_tasks for uav_id in task['uav_ids']))

            # 解析当前组的任务目标
            for task in group_tasks:
                if task['taskType'] == 'patrol':
                    self._prepare_patrol_goals(task['vertices'], task['uav_ids'], self.target_patrol_time)
                elif task['taskType'] == 'waypoint':
                    self._prepare_waypoint_goals(task['goals'], task['uav_ids'])

            # 执行规划
            approach_trajectories, grid_time_paths, approach_world_paths, planner = self._execute_planning(
                uavs_in_group, task_type=f"priority_{priority}_group", reservation_table=self.reservation_table
            )

            # 存储规划器
            for uav_id in uavs_in_group:
                if uav_id in approach_world_paths:
                    self.uav_planner[uav_id] = planner

            # 拼接巡航路径
            group_final_trajs, group_final_grid, group_final_world = self._stitch_patrol_paths(
                approach_trajectories, grid_time_paths, approach_world_paths, planner
            )

            # 合并到全局结果
            for uav_id, path in group_final_grid.items():
                if path:
                    self.reservation_table.reserve_path(path)
                    self.all_time_paths[uav_id] = path
            all_uav_trajectories.update(group_final_trajs)
            final_world_paths.update(group_final_world)

        self._prepare_animation_timestamps()
        return all_uav_trajectories, final_world_paths

    def _process_multi_waypoint_tasks(self, tasks_json: List[Dict]):
        """处理多段途径点任务"""
        all_uav_trajectories = {}
        all_world_paths = {}
        
        for task in tasks_json:
            if task['taskType'] == 'multi_waypoint':
                uav_id = task['uav_ids'][0]
                waypoint_sequence = task['waypoint_sequence']
                time_constraint = task['time_constraint']
                
                print(f"\n=== 开始多段规划 UAV {uav_id} ===")
                print(f"航点序列: {len(waypoint_sequence)} 个点")
                
                # 执行多段规划
                gps_trajectory, world_path = self._execute_multi_waypoint_planning(
                    uav_id, waypoint_sequence, time_constraint
                )
                
                if gps_trajectory and world_path:
                    all_uav_trajectories[uav_id] = gps_trajectory
                    all_world_paths[uav_id] = world_path
                    print(f"UAV {uav_id} 多段规划完成，总点数: {len(gps_trajectory)}")
                else:
                    print(f"UAV {uav_id} 多段规划失败")
            
            elif task['taskType'] in ['waypoint', 'patrol']:
                # 处理原有的单段任务
                gps_trajs, world_paths = self._process_single_segment_tasks([task])
                all_uav_trajectories.update(gps_trajs)
                all_world_paths.update(world_paths)
        
        return all_uav_trajectories, all_world_paths
    
    def _execute_planning(self, uav_ids: List[int], task_type: str, reservation_table: ReservationTable) -> Tuple[Dict, Dict, Dict, SpaceTimeAStar3D]:
        """
        执行规划，返回：GPS航点、栅格路径、世界路径、规划器
        1）用当前 self.speed（按 per-UAV 上限截断）规划一次；
        2）如果规划失败（无路径 / 数量不匹配），或不满足时间约束则直接调用 _replan_on_fail()，用本组 UAV 的最大允许速度（group_max_allowed）再规划一次；
        """
        trajectories_gps = {}  # GPS航点（最终输出）
        grid_paths = {}        # 栅格时空路径（用于预留表）
        world_paths = {}       # 世界坐标路径（返回给拼接用）
        current_planning_speed = self.speed
        planner = None
        # 1. 准备规划输入（无人机起点+目标点，局部世界坐标）
        uav_list = []
        active_uav_ids = []
        for uid in uav_ids:
            start_world = self.uav_positions.get(uid)
            goal_world = self.uav_goals.get(uid)
            if start_world is None or goal_world is None:
                print(f"UAV {uid} has incomplete data (start/goal missing), skipping.")
                continue
            uav_list.append((start_world[0], start_world[1], start_world[2],
                             goal_world[0], goal_world[1], goal_world[2]))
            active_uav_ids.append(uid)

        if not uav_list:
            print("No valid UAV data for planning.")
            return {}, {}, {}, None

        # 2. 计算本次规划涉及无人机的速度上限（取每机上限中的最小值作为组上限）
        try:
            group_max_allowed = min([
                self.uav_max_planning_speed.get(uid, self.max_planning_speed)
                for uid in active_uav_ids
            ])
        except Exception:
            group_max_allowed = self.max_planning_speed

        # 当前尝试速度 = min(self.speed, 组上限)
        current_planning_speed = min(self.speed, group_max_allowed)

        print(f"\n--- Planning (single attempt) for {task_type} with speed: {current_planning_speed:.2f} m/s (group cap {group_max_allowed:.2f}) ---")
        print(f"Calling multi_uav_planning_3d with speed={current_planning_speed:.2f} m/s for UAVs {active_uav_ids}")

        # 3. 执行一次规划
        grid_raw, planner = multi_uav_planning_3d(
            uav_list, self.map_env, reservation_table,
            current_planning_speed, safe_dis=self.safe_dis, max_t=self.max_t,
            heuristic_weight=self.heuristic_weight
        )

        # 3.1 若规划失败，直接走 fallback，用组最大速度重试一次
        if not grid_raw or len(grid_raw) != len(uav_list):
            print("Planning failed at base speed. Fallback with group max speed.")
            final_gps, final_grid, final_world = self._replan_on_fail(
                uav_ids, group_max_allowed, reservation_table
            )
            return final_gps, final_grid, final_world, planner

        # 4. 栅格路径→世界坐标（立即转换）并做高度修正
        world_paths_list = []  # 每个无人机的世界坐标路径
        for grid_path in grid_raw:
            if not grid_path:
                world_paths_list.append([])
                continue
            world_path = self.grid_path_to_world(grid_path, planner)
            world_paths_list.append(world_path)

        valid_planning = True
        for i, uid in enumerate(active_uav_ids):
            world_path = world_paths_list[i]
            if not world_path:
                valid_planning = False
                break

            # 修正 1：将开头连续 z==0 的点替换为第一个非零高度
            first_nonzero_z = None
            for (_x, _y, _z, _t) in world_path:
                if _z is not None and _z != 0:
                    first_nonzero_z = _z
                    break
            if first_nonzero_z is not None:
                for j, (xw, yw, zw, tw) in enumerate(world_path):
                    if zw == 0:
                        world_path[j] = (xw, yw, first_nonzero_z, tw)
                    else:
                        break

            # 修正 2：用真实 start/goal 的 XYZ 覆盖首尾
            start_world = self.uav_positions.get(uid)
            goal_world = self.uav_goals.get(uid)
            if world_path:
                sx, sy, sz, st = world_path[0]
                if start_world is not None:
                    world_path[0] = (start_world[0], start_world[1], start_world[2], st)
                ex, ey, ez, et = world_path[-1]
                if goal_world is not None:
                    world_path[-1] = (goal_world[0], goal_world[1], goal_world[2], et)

            # 修正 3：如果路径高度与目标高度相差在一个阈值内，路径高度替换为目标高度
            if world_path and goal_world is not None:
                goal_altitude = goal_world[2]
                for j, (x, y, z, t) in enumerate(world_path):
                    if z-goal_altitude > -planner.resolution/2 and z-goal_altitude < planner.resolution/2:
                        world_path[j] = (x, y, goal_altitude, t)

            world_paths[uid] = world_path

        # 若有任意一架机路径后处理失败 → fallback
        if not valid_planning:
            print("Path post-processing failed. Fallback with group max speed.")
            final_gps, final_grid, final_world = self._replan_on_fail(
                uav_ids, group_max_allowed, reservation_table
            )
            return final_gps, final_grid, final_world, planner

        # 5. 检查时间约束：不满足则直接 fallback（不再调速重试）
        needs_fallback = False
        for uid in active_uav_ids:
            if uid not in world_paths:
                needs_fallback = True
                break
            path_time = world_paths[uid][-1][3] - world_paths[uid][0][3]
            task_time = self.task_time_constraints.get(uid, float('inf'))
            cruise_time = self.cruise_data.get(uid, {}).get("estimated_time", 0.0)
            available_time = task_time - self.time_buffer - cruise_time
            available_time = max(0.1, available_time)

            print(f"UAV {uid}: path time={path_time:.2f}s, available time={available_time:.2f}s")
            if path_time > available_time:
                print(f"Time constraint violated for UAV {uid}: ratio={path_time/available_time:.2f} > 1.0")
                needs_fallback = True
                break

        if needs_fallback:
            print("Time constraint not met. Fallback with group max speed.")
            final_gps, final_grid, final_world = self._replan_on_fail(
                uav_ids, group_max_allowed, reservation_table
            )
            return final_gps, final_grid, final_world, planner

        # 6. 正常成功：生成 GPS + 栅格，并记录规划速度
        for uid in active_uav_ids:
            self.final_planning_speeds[uid] = current_planning_speed

        for uid in active_uav_ids:
            world_path = world_paths[uid]
            # 6.1 世界坐标路径→GPS航点
            gps_waypoints = []
            for (x_w, y_w, z_w, _) in world_path:
                lat, lon = self.pos_converter.XYtoGPS(x_w, y_w, self.ref_lat, self.ref_lon)
                gps_waypoints.append({"lat": lat, "lon": lon, "alt": z_w})
            trajectories_gps[uid] = gps_waypoints

            # 6.2 世界坐标路径→栅格索引（用于预留表）
            grid_path = self.world_path_to_grid(world_path, planner)
            grid_paths[uid] = grid_path

        return trajectories_gps, grid_paths, world_paths, planner

    def _execute_multi_waypoint_planning(self, uav_id: int, waypoint_sequence: List[Dict], time_constraint: float):
        """执行多段途径点规划，确保时间戳连续性"""
        all_segment_gps = []
        all_segment_world = []
        current_time_offset = 0.0
        # 存储规划器实例（用于可视化）
        planner_instance = None
        
        # 设置初始位置
        start_point = waypoint_sequence[0]
        x, y = self.pos_converter.GPStoXY(start_point['lat'], start_point['lon'], self.ref_lat, self.ref_lon)
        self.uav_positions[uav_id] = (x, y, start_point['alt'])
        self.task_time_constraints[uav_id] = time_constraint
        
        # 分段规划，多次将目标设置为下一个航点
        for i in range(len(waypoint_sequence) - 1):
            start_wp = waypoint_sequence[i]
            end_wp = waypoint_sequence[i + 1]
            
            print(f"规划段 {i+1}/{len(waypoint_sequence)-1}: {start_wp['lat']:.4f},{start_wp['lon']:.4f} -> "
                f"{end_wp['lat']:.4f},{end_wp['lon']:.4f}")
            
            # 设置当前段的目标
            x_goal, y_goal = self.pos_converter.GPStoXY(end_wp['lat'], end_wp['lon'], self.ref_lat, self.ref_lon)
            self.uav_goals[uav_id] = (x_goal, y_goal, end_wp['alt'])
            
            # 执行单段规划
            # 在多段规划中，确保每段的规划速度不超过该 UAV 的能力上限
            per_uav_cap = self.uav_max_planning_speed.get(uav_id, self.max_planning_speed)
            # 暂时降低 self.speed 以作为 _execute_planning 的初始速度（后续函数会根据组上限调整）
            old_speed = self.speed
            try:
                self.speed = min(self.speed, per_uav_cap)
                print(f"Calling segment planner for UAV {uav_id} with per-UAV cap {per_uav_cap:.2f}, effective self.speed={self.speed:.2f}")
                gps_trajs, grid_paths, world_paths, planner = self._execute_planning(
                    [uav_id], task_type=f"segment_{i+1}", reservation_table=self.reservation_table
                )
                print(f"Segment planner returned for UAV {uav_id}, used planning speed approx {self.final_planning_speeds.get(uav_id, self.speed):.2f}")
            finally:
                self.speed = old_speed
            # 保存规划器实例（使用最后一个有效的规划器）
            if planner is not None:
                planner_instance = planner

            if uav_id in gps_trajs and uav_id in world_paths:
                segment_gps = gps_trajs[uav_id]
                segment_world = world_paths[uav_id]
                
                if segment_world:
                    # 调整时间戳：后续段的时间要接续前一段
                    adjusted_segment_world = []
                    for point in segment_world:
                        x, y, z, t = point
                        adjusted_segment_world.append((x, y, z, t + current_time_offset))
                    
                    # 更新当前时间偏移（使用调整后的最后时间）
                    current_time_offset = adjusted_segment_world[-1][3] if adjusted_segment_world else current_time_offset
                    
                    # 拼接路径（第一段全加，后续段跳过第一个点避免重复）
                    if i == 0:
                        all_segment_gps.extend(segment_gps)
                        all_segment_world.extend(adjusted_segment_world)
                    else:
                        all_segment_gps.extend(segment_gps[1:])
                        all_segment_world.extend(adjusted_segment_world[1:])
                    
                    # 更新预留表
                    if uav_id in grid_paths and grid_paths[uav_id]:
                        adjusted_grid_path = []
                        for ix, iy, iz, t in grid_paths[uav_id]:
                            adjusted_grid_path.append((ix, iy, iz, t + current_time_offset))
                        self.reservation_table.reserve_path(adjusted_grid_path)
                        self.all_time_paths[uav_id] = adjusted_grid_path
                    
                    print(f"段 {i+1} 规划成功，点数: {len(segment_gps)}，累计时间: {current_time_offset:.2f}s")
                else:
                    print(f"段 {i+1} 规划失败：空路径")
                    return None, None
            else:
                print(f"段 {i+1} 规划失败")
                return None, None
            
            # 更新下一段的起点为当前段的终点
            self.uav_positions[uav_id] = self.uav_goals[uav_id]
        
            # 关键修复：存储规划器实例用于可视化
        if planner_instance is not None:
            self.uav_planner[uav_id] = planner_instance
            print(f"已为无人机 {uav_id} 存储规划器实例")
        else:
            print(f"警告: 无人机 {uav_id} 没有找到有效的规划器实例")
        return all_segment_gps, all_segment_world
    
    def _prepare_animation_timestamps(self):
        """收集并排序所有无人机的时间戳"""
        if not self.all_time_paths:
            return
        timestamps = set()
        for uid, path in self.all_time_paths.items():
            for _, _, _, t in path:
                timestamps.add(round(t, 3))
        self.animation_timestamps = sorted(timestamps)
        if self.animation_timestamps:
            print(f"\nAnimation Info: {len(self.animation_timestamps)} frames, Duration: {self.animation_timestamps[-1]:.2f}s")

    def _process_start_positions(self, start_positions: List[Dict]):
        """处理无人机起始位置（GPS→局部世界坐标）"""
        for start in start_positions:
            uav_id = start['uav_id']
            if uav_id in self.uav_positions:
                continue
            pos = start['position']
            lat, lon, alt = pos['lat'], pos['lon'], pos['alt']
            x, y = self.pos_converter.GPStoXY(lat, lon, self.ref_lat, self.ref_lon)
            self.uav_positions[uav_id] = (x, y, alt)
            print(f"UAV {uav_id} start local coordinates: (x={x:.2f}, y={y:.2f}, alt={alt:.2f})")

    def _prepare_waypoint_goals(self, goals: List[Dict], uav_ids: List[int]):
        """准备航点任务目标（GPS→局部世界坐标）"""
        for goal in goals:
            uav_id = goal['uav_id']
            if uav_id not in uav_ids:
                continue
            pos = goal['position']
            lat, lon, alt = pos['lat'], pos['lon'], pos['alt']
            x, y = self.pos_converter.GPStoXY(lat, lon, self.ref_lat, self.ref_lon)
            self.uav_goals[uav_id] = (x, y, alt)
            print(f"UAV {uav_id} waypoint goal set to: (x={x:.2f}, y={y:.2f}, alt={alt:.2f})")

    def _prepare_patrol_goals(self, vertices: List[Dict], uav_ids: List[int], patrol_time: float):
        """准备巡航任务目标（入口点+巡航路径，GPS→局部世界坐标）"""
        xy_vertices = []
        patrol_altitude = self.default_altitude
        target_patrol_time = patrol_time
        # 取区域顶点的最大海拔作为巡航高度
        if vertices:
            max_alt = max([v['alt'] for v in vertices if v['alt'] != 0.0], default=0.0)
            patrol_altitude = max_alt if max_alt > 0 else self.default_altitude
        # 转换区域顶点为局部世界坐标
        for v in vertices:
            x, y = self.pos_converter.GPStoXY(v['lat'], v['lon'], self.ref_lat, self.ref_lon)
            xy_vertices.append({"x": x, "y": y, "z": patrol_altitude})
        # 生成巡航路径
        generated_goals, patrol_path_points = self._generate_patrol_goals(
            xy_vertices, uav_ids, patrol_altitude, self.scan_speed
        )

        if not generated_goals:
            print(f"Error: Failed to generate patrol goals for UAVs {uav_ids}.")
            return

        # 根据期望巡航时间调整速度
        for goal in generated_goals:
            uav_id = goal["uav_id"]
            entry_point_world = (goal["position"]["x"], goal["position"]["y"], goal["position"]["z"])

            # 获取该无人机的巡航路径
            uav_patrol_points = patrol_path_points.get(uav_id, [])
            if not uav_patrol_points:
                continue

            # 计算巡航路径总长度
            total_patrol_length = 0.0
            for i in range(len(uav_patrol_points) - 1):
                p1 = uav_patrol_points[i]
                p2 = uav_patrol_points[i + 1]
                total_patrol_length += math.hypot(p2[0] - p1[0], p2[1] - p1[1])

            # 根据期望巡航时间计算所需速度
            required_speed = total_patrol_length / target_patrol_time

            # 获取该无人机的最大巡航速度（若任务有单独限制则优先使用）
            per_uav_max = self.uav_max_patrol_speed.get(uav_id, self.max_patrol_speed)
            # 限制速度在合理范围内（使用 per-uav 上限）
            adjusted_speed = np.clip(required_speed, self.min_patrol_speed, per_uav_max)

            # 计算实际能达到的巡航时间
            actual_patrol_time = total_patrol_length / adjusted_speed

            print(f"UAV {uav_id}: 期望巡航时间={target_patrol_time}s, 所需速度={required_speed:.2f}m/s, "
                f"限制速度后={adjusted_speed:.2f}m/s (max {per_uav_max:.2f}), 实际巡航时间={actual_patrol_time:.2f}s")

            if required_speed > per_uav_max:
                print(f"警告: UAV {uav_id} 需要速度 {required_speed:.2f}m/s 超过最大限制，使用最大速度 {per_uav_max}m/s")
            elif required_speed < self.min_patrol_speed:
                print(f"警告: UAV {uav_id} 需要速度 {required_speed:.2f}m/s 低于最小限制，使用最小速度 {self.min_patrol_speed}m/s")

            cruise_speed = adjusted_speed
            estimated_patrol_time = actual_patrol_time
        # 转换巡航路径为GPS
            converted_gps_path = []
            for point_world in uav_patrol_points:
                lat_wp, lon_wp = self.pos_converter.XYtoGPS(
                    point_world[0], point_world[1], self.ref_lat, self.ref_lon
                )
                converted_gps_path.append({"lat": lat_wp, "lon": lon_wp, "alt": patrol_altitude})

            # 存储巡航数据
            self.uav_goals[uav_id] = entry_point_world
            self.cruise_data[uav_id] = {
                "entry_point_world": entry_point_world,
                "path_gps": converted_gps_path,
                "path_world": uav_patrol_points,
                "altitude": patrol_altitude,
                "estimated_time": estimated_patrol_time,
                "speed": cruise_speed,
                "target_patrol_time": target_patrol_time  # 记录期望时间
            }
            self.final_patrol_speeds[uav_id] = cruise_speed
            
            print(f"UAV {uav_id} 巡航配置: 速度={cruise_speed:.2f}m/s, 预估时间={estimated_patrol_time:.2f}s")


    def _stitch_patrol_paths(self, approach_trajectories: Dict, grid_time_paths: Dict, 
                             approach_world_paths: Dict, planner) -> Tuple[Dict, Dict, Dict]:
        """
        修正核心：拼接"接近路径"与"巡航路径"，先按uav_id取列表再切片
        返回：最终GPS轨迹、最终栅格路径、最终世界坐标路径
        """
        final_trajectories = {}
        final_grid_paths = {}
        final_world_paths = {}

        for uav_id, approach_gps in approach_trajectories.items():
            # 1. 获取当前无人机的接近世界路径（字典→列表）
            current_approach_world = approach_world_paths.get(uav_id, [])  # 关键修正：按uav_id取列表
            current_grid = grid_time_paths.get(uav_id, [])

            # 2. 无巡航任务：直接沿用接近路径
            if uav_id not in self.cruise_data:
                final_trajectories[uav_id] = approach_gps
                final_grid_paths[uav_id] = current_grid
                final_world_paths[uav_id] = current_approach_world
                print(f"UAV {uav_id} (no patrol) - use approach path (len: {len(current_approach_world)})")
                continue

            # 3. 有巡航任务：拼接路径（核心逻辑修正）
            cruise_info = self.cruise_data[uav_id]
            cruise_world_raw = cruise_info["path_world"]  # 巡航原始路径（无时间戳）
            cruise_speed = cruise_info["speed"]

            # 3.1 校验接近路径是否为空
            if not current_approach_world:
                print(f"Warning: UAV {uav_id} approach path is empty, skip stitching")
                final_trajectories[uav_id] = approach_gps
                final_grid_paths[uav_id] = current_grid
                final_world_paths[uav_id] = current_approach_world
                continue

            # 3.2 为巡航路径添加时间戳（基于接近路径的结束时间）
            last_approach_time = current_approach_world[-1][3]  # 接近路径最后一个点的时间
            cruise_world_with_time = []
            current_time = last_approach_time  # 从接近路径结束时间开始计时

            for i, (x, y, z) in enumerate(cruise_world_raw):
                # 添加当前点（含时间戳）
                cruise_world_with_time.append((x, y, z, current_time))
                # 计算下一个点的时间（避免最后一个点重复计算）
                if i < len(cruise_world_raw) - 1:
                    next_x, next_y = cruise_world_raw[i + 1][0], cruise_world_raw[i + 1][1]
                    dist = math.hypot(next_x - x, next_y - y)
                    current_time += dist / cruise_speed if cruise_speed > 0 else 0.1  # 避免除以0

            # 3.3 拼接世界坐标路径（接近路径切片 + 巡航路径）
            # 关键修正：对接近路径列表切片[:-1]，删除最后一个点避免与巡航起点重复
            merged_world = current_approach_world[:-1] + cruise_world_with_time
            final_world_paths[uav_id] = merged_world

            # 3.4 拼接GPS航点（接近GPS + 巡航GPS，跳过巡航第一个点）
            cruise_gps = cruise_info["path_gps"]
            merged_gps = approach_gps + cruise_gps[1:]  # 跳过巡航第一个点（与接近路径终点重复）
            final_trajectories[uav_id] = merged_gps

            # 3.5 拼接栅格路径（接近栅格 + 巡航栅格）
            # 转换巡航世界路径为栅格索引
            cruise_grid = self.world_path_to_grid(cruise_world_with_time, planner)
            # 接近栅格切片[:-1]，删除最后一个点
            merged_grid = current_grid[:-1] + cruise_grid
            final_grid_paths[uav_id] = merged_grid

            # 打印拼接结果
            print(f"UAV {uav_id} (patrol) - stitched:")
            print(f"  Approach world points: {len(current_approach_world)} → sliced to {len(current_approach_world[:-1])}")
            print(f"  Cruise world points: {len(cruise_world_with_time)}")
            print(f"  Merged world points: {len(merged_world)}")

        return final_trajectories, final_grid_paths, final_world_paths

    def _generate_patrol_goals(self, vertices: List[Dict], uav_ids: List[int], altitude: float, scan_speed: float) -> Tuple[List[Dict], Dict]:
        """生成巡航入口点和巡航路径（区域覆盖算法）"""
        try:
            # 准备无人机当前位置（局部世界坐标）
            uav_current_world = []
            for uav_id in uav_ids:
                x, y, _ = self.uav_positions.get(uav_id, (0.0, 0.0, altitude))
                uav_current_world.append([x, y, altitude])
            # 准备区域顶点（局部世界坐标）
            polygon_world = [[v["x"], v["y"], v["z"]] for v in vertices]

            # 调用区域覆盖规划器
            planner = AreaCoveragePlanner(
                uav_ids=uav_ids,
                polygon_vertices=polygon_world,
                uav_current_positions=uav_current_world,
                flight_altitude=altitude,
                sweep_width=self.sweep_width,
                scan_speed=scan_speed,
                turn_speed=scan_speed * 0.5,
                turn_radius_factor=0.4,
                path_boundary_standoff=self.boundary_standoff,
                max_segment_length=self.max_segment_length,
                uav_endurance_time_sec=self.max_t * 2,
                uav_battery_reserve_factor=0.2,
                uav_setup_time_sec=0,
                sensor_overlap_factor=0.0,
                random_seed=42
            )
            path_world, _ = planner.plan_routes()  # path_world: {uav_id: [(x,y,z), ...]}

            # 提取每个无人机的巡航入口点（巡航路径第一个点）
            generated_goals = []
            for uav_id in uav_ids:
                if uav_id in path_world and len(path_world[uav_id]) > 0:
                    first_point = path_world[uav_id][0]
                    generated_goals.append({
                        "uav_id": uav_id,
                        "position": {"x": first_point[0], "y": first_point[1], "z": first_point[2]}
                    })
                else:
                    print(f"Area coverage planner failed to generate path for UAV {uav_id}.")

            return generated_goals, path_world

        except Exception as e:
            print(f"Area coverage planning failed: {str(e)}. Returning empty goals and paths.")
            return [], {}


    def _replan_on_fail(self, uav_ids: List[int], final_speed: float, reservation_table: ReservationTable) -> Tuple[Dict, Dict, Dict]:
        """规划失败时的降级策略：用最大速度重试，仅保证路径可行"""
        print(f"Executing fallback planning with speed {final_speed:.2f} m/s.")
        final_gps = {}
        final_grid = {}
        final_world = {}

        # 准备规划输入
        uav_list = []
        active_uav_ids = []
        for uid in uav_ids:
            start_world = self.uav_positions.get(uid)
            goal_world = self.uav_goals.get(uid)
            if start_world and goal_world:
                uav_list.append((start_world[0], start_world[1], start_world[2], goal_world[0], goal_world[1], goal_world[2]))
                active_uav_ids.append(uid)
        if not uav_list:
            return final_gps, final_grid, final_world

        # 规划
        grid_raw, planner = multi_uav_planning_3d(
            uav_list, self.map_env, reservation_table,
            final_speed, safe_dis=self.safe_dis, max_t=self.max_t
            , heuristic_weight=self.heuristic_weight
        )
        if not grid_raw:
            return final_gps, final_grid, final_world

        # 转换和处理路径
        for i, uid in enumerate(active_uav_ids):
            if i >= len(grid_raw):
                continue
            grid_path = grid_raw[i]
            if not grid_path:
                continue
            world_path = self.grid_path_to_world(grid_path, planner)
            
            # ------ 修正 1: 替换开头连续 z==0 ------
            if world_path:
                first_nonzero_z = None
                for (_x, _y, _z, _t) in world_path:
                    if _z is not None and _z != 0:
                        first_nonzero_z = _z
                        break
                if first_nonzero_z is not None:
                    for j, (xw, yw, zw, tw) in enumerate(world_path):
                        if zw == 0:
                            world_path[j] = (xw, yw, first_nonzero_z, tw)
                        else:
                            break

            # ------ 修正 2: 用真实 start/goal 的 XYZ 覆盖首尾 ------
            start_world = self.uav_positions.get(uid)
            goal_world = self.uav_goals.get(uid)
            if world_path:
                sx, sy, sz, st = world_path[0]
                if start_world is not None:
                    world_path[0] = (start_world[0], start_world[1], start_world[2], st)
                ex, ey, ez, et = world_path[-1]
                if goal_world is not None:
                    world_path[-1] = (goal_world[0], goal_world[1], goal_world[2], et)
            # ------ 修正 3: 如果路径高度大于目标高度，修正为目标高度 ------
            if world_path and goal_world is not None:
                goal_altitude = goal_world[2]
                for j, (x, y, z, t) in enumerate(world_path):
                    if z > goal_altitude:
                        world_path[j] = (x, y, goal_altitude, t)        
            final_world[uid] = world_path
            # 转GPS和栅格
            gps = []
            for (x, y, z, _) in world_path:
                lat, lon = self.pos_converter.XYtoGPS(x, y, self.ref_lat, self.ref_lon)
                gps.append({"lat": lat, "lon": lon, "alt": z})
            final_gps[uid] = gps
            final_grid[uid] = self.world_path_to_grid(world_path, planner)
            

        self.final_planning_speeds = {uid: final_speed for uid in active_uav_ids}
        return final_gps, final_grid, final_world

    def grid_path_to_world(self, path_indices: List[Tuple[int, int, int, float]], planner: SpaceTimeAStar3D) -> List[Tuple[float, float, float, float]]:
        """栅格索引路径 → 局部世界坐标路径"""
        if not path_indices or not planner:
            return []
        world_path = []
        for ix, iy, iz, t in path_indices:
            # 栅格索引→世界坐标（调用规划器自带方法）
            x_w = planner.calc_grid_position(ix, planner.min_x)
            y_w = planner.calc_grid_position(iy, planner.min_y)
            z_w = planner.calc_grid_position(iz, planner.min_z)
            world_path.append((x_w, y_w, z_w, t))
        return world_path

    def world_path_to_grid(self, world_path: List[Tuple[float, float, float, float]], planner: SpaceTimeAStar3D) -> List[Tuple[int, int, int, float]]:
        """局部世界坐标路径 → 栅格索引路径"""
        if not world_path or not planner:
            return []
        grid_path = []
        for x_w, y_w, z_w, t in world_path:
            # 世界坐标→栅格索引（调用规划器自带方法）
            ix = planner.calc_xyz_index(x_w, planner.min_x)
            iy = planner.calc_xyz_index(y_w, planner.min_y)
            iz = planner.calc_xyz_index(z_w, planner.min_z)
            # 确保索引在地图范围内（避免越界）
            ix = np.clip(ix, 0, planner.x_width - 1)
            iy = np.clip(iy, 0, planner.y_width - 1)
            iz = np.clip(iz, 0, planner.z_width - 1)
            grid_path.append((ix, iy, iz, t))
        return grid_path

    def _add_speed_to_gps_trajectory(self, gps_traj: List[Dict], world_path: List[Tuple[float, float, float, float]], uav_id: Optional[int]=None) -> List[Dict]:
        """
        给GPS轨迹添加建议飞行速度（基于世界路径的时间戳与距离）
        
        Args:
            gps_traj: 原始GPS轨迹（含lat/lon/alt）
            world_path: 对应的世界坐标路径（含x/y/z/t）
        
        Returns:
            带速度的GPS轨迹（新增speed字段）
        """
        if not gps_traj or not world_path or len(gps_traj) != len(world_path):
            print("GPS轨迹与世界路径长度不匹配，无法添加速度")
            return gps_traj
        
        # 初始化带速度的轨迹（第一个航点速度为0）
        gps_with_speed = []
        for i, (gps_point, world_point) in enumerate(zip(gps_traj, world_path)):
            x, y, z, t = world_point
            new_gps_point = gps_point.copy()
            
            if i == 0:
                # 第一个航点：静止状态，速度为0
                new_gps_point["speed"] = 0.0
            elif i == len(gps_traj) - 1:
                # 最后一个航点：终点悬停，速度为0
                new_gps_point["speed"] = 0.0
            else:
                # 计算当前点与前一个点的距离和时间差
                prev_x, prev_y, prev_z, prev_t = world_path[i-1]
                # 3D欧氏距离
                distance = math.hypot(
                    math.hypot(x - prev_x, y - prev_y),  # 水平距离
                    z - prev_z                           # 垂直距离
                )
                time_diff = t - prev_t
                
                # 避免时间差为0（防止除以0）
                if time_diff <= 1e-6:  # 时间差小于1微秒，视为同一时刻
                    new_gps_point["speed"] = self.speed  # 使用规划基础速度
                    print(f"警告：航点{i-1}与{i}时间差过小，使用基础速度{self.speed} m/s")
                else:
                    # 计算平均速度（m/s），保留2位小数
                    speed = round(distance / time_diff, 2)
                    # 使用 per-UAV 上限（如果提供 uav_id）
                    per_uav_cap = self.max_planning_speed
                    if uav_id is not None:
                        per_uav_cap = self.uav_max_planning_speed.get(uav_id, self.max_planning_speed)
                    speed = min(speed, per_uav_cap)
                    new_gps_point["speed"] = speed
            
            gps_with_speed.append(new_gps_point)
        
        return gps_with_speed
    # ---------------------- 世界坐标路径可视化方法 ----------------------
    def plot_static_world_trajectories_3d(self, all_world_paths: Dict[int, List[Tuple[float, float, float, float]]]):
        """静态3D图：绘制所有无人机的世界坐标路径，标记起点、终点和障碍物（修复多空图问题）"""
        # 第一步：过滤掉空路径，只保留有有效轨迹的无人机数据
        valid_world_paths = {}
        for uav_id, path in all_world_paths.items():
            if path and len(path) > 0:  # 只保留非空且长度>0的路径
                valid_world_paths[uav_id] = path
        
        # 若无有效路径，直接返回（避免创建空图）
        if not valid_world_paths:
            print("No valid world paths to plot (static 3D).")
            return
        
        # 第二步：基于有效路径获取规划器（避免取到空路径对应的无效规划器）
        sample_uav_id = next(iter(valid_world_paths.keys()))  # 从有效路径中取第一个无人机ID
        planner = self.uav_planner.get(sample_uav_id)
        if not planner:
            print("No planner found (static 3D plot failed).")
            return

        # 第三步：只创建一张3D图，统一绘制所有有效轨迹
        fig = plt.figure(figsize=(10, 8))
        ax = fig.add_subplot(111, projection='3d')
        ax.set_title('Multi-UAV World Coordinate Trajectories (Static 3D)', fontsize=12)
        ax.set_xlabel('X [m]', fontsize=10)
        ax.set_ylabel('Y [m]', fontsize=10)
        ax.set_zlabel('Z [m]', fontsize=10)

        # 1. 绘制障碍物（基于有效路径对应的规划器，确保地图数据正确）
        obs_x, obs_y, obs_z = [], [], []
        for ix in range(planner.x_width):
            for iy in range(planner.y_width):
                for iz in range(planner.z_width):
                    if planner.obs_map[ix][iy][iz]:
                        x = planner.calc_grid_position(ix, planner.min_x)
                        y = planner.calc_grid_position(iy, planner.min_y)
                        z = planner.calc_grid_position(iz, planner.min_z)
                        obs_x.append(x)
                        obs_y.append(y)
                        obs_z.append(z)
        if obs_x:
            ax.scatter(obs_x, obs_y, obs_z, c='gray', s=8, alpha=0.5, label='Obstacles')

        # 2. 只绘制有效路径（避免循环空路径创建空图）
        colors = plt.get_cmap('tab10')(range(len(valid_world_paths)))
        for idx, (uav_id, world_path) in enumerate(valid_world_paths.items()):
            # 提取路径坐标（此时world_path已确保非空）
            xs = [p[0] for p in world_path]
            ys = [p[1] for p in world_path]
            zs = [p[2] for p in world_path]

            # 绘制路径线、起点、终点
            ax.plot(xs, ys, zs, linewidth=2.5, color=colors[idx], label=f'UAV {uav_id}')
            ax.scatter(xs[0], ys[0], zs[0], marker='o', s=100, c='red', edgecolors=colors[idx], linewidth=1.5)
            ax.scatter(xs[-1], ys[-1], zs[-1], marker='^', s=120, c='blue', edgecolors=colors[idx], linewidth=1.5)

        # 调整图像显示
        ax.legend(loc='best', fontsize=9)
        ax.view_init(elev=20, azim=45)
        plt.tight_layout()
        print("\nShowing static 3D trajectory plot (close window to continue to animation)...")
        plt.show()

    def animate_world_trajectories_3d(self, all_world_paths: Dict[int, List[Tuple[float, float, float, float]]], interval=100):
        """动态3D动画：显示无人机实时位置和轨迹尾迹，同步显示时间"""
        TIME_SCALE = 100     # 时间离散比例：真实时间 * TIME_SCALE -> 整数刻度
        SUBSTEPS = 5         # 动画插值步数
        if not all_world_paths:
            print("No world paths to animate (3D animation).")
            return
        # 获取规划器和无人机列表
        sample_uav_id = next(iter(all_world_paths.keys()))
        planner = self.uav_planner.get(sample_uav_id)
        uav_ids = list(all_world_paths.keys())
        if not planner:
            print("No planner found (3D animation failed).")
            return

        # 1. 预处理：为每个无人机构建"时间→位置"映射
        uav_keyframes = {}  # {uav_id: {time_tick: (x,y,z)}}
        all_time_ticks = set()
        for uav_id, world_path in all_world_paths.items():
            keyframes = {}
            for (x, y, z, t) in world_path:
                time_tick = int(round(t * TIME_SCALE))  # 时间缩放为整数帧
                keyframes[time_tick] = (x, y, z)
                all_time_ticks.add(time_tick)
            uav_keyframes[uav_id] = keyframes

        # 2. 生成平滑的时间轴（插值补帧）
        sorted_ticks = sorted(all_time_ticks)
        if len(sorted_ticks) < 2:
            sorted_ticks = sorted_ticks + [sorted_ticks[0] + 1]  # 避免单帧无法动画
        smooth_ticks = []
        for i in range(len(sorted_ticks) - 1):
            t0, t1 = sorted_ticks[i], sorted_ticks[i+1]
            # 插值中间帧
            for step in range(SUBSTEPS):
                smooth_ticks.append(t0 + (t1 - t0) * step / SUBSTEPS)
        smooth_ticks.append(sorted_ticks[-1])  # 加上最后一帧

        # 3. 为每个无人机生成平滑轨迹（线性插值）
        uav_smooth_trajs = {}  # {uav_id: (xs, ys, zs)}
        for uav_id in uav_ids:
            keyframes = uav_keyframes[uav_id]
            ticks = np.array(sorted(keyframes.keys()), dtype=float)
            xs = np.array([keyframes[t][0] for t in ticks])
            ys = np.array([keyframes[t][1] for t in ticks])
            zs = np.array([keyframes[t][2] for t in ticks])
            # 线性插值到平滑时间轴
            smooth_x = np.interp(smooth_ticks, ticks, xs)
            smooth_y = np.interp(smooth_ticks, ticks, ys)
            smooth_z = np.interp(smooth_ticks, ticks, zs)
            uav_smooth_trajs[uav_id] = (smooth_x, smooth_y, smooth_z)

        # 4. 创建动画图
        fig = plt.figure(figsize=(10, 8))
        ax = fig.add_subplot(111, projection='3d')
        ax.set_title('Multi-UAV World Coordinate Trajectories (3D Animation)', fontsize=12)
        ax.set_xlabel('X [m]', fontsize=10)
        ax.set_ylabel('Y [m]', fontsize=10)
        ax.set_zlabel('Z [m]', fontsize=10)

        # 绘制障碍物（与静态图一致）
        obs_x, obs_y, obs_z = [], [], []
        for ix in range(planner.x_width):
            for iy in range(planner.y_width):
                for iz in range(planner.z_width):
                    if planner.obs_map[ix][iy][iz]:
                        x = planner.calc_grid_position(ix, planner.min_x)
                        y = planner.calc_grid_position(iy, planner.min_y)
                        z = planner.calc_grid_position(iz, planner.min_z)
                        obs_x.append(x)
                        obs_y.append(y)
                        obs_z.append(z)
        if obs_x:
            ax.scatter(obs_x, obs_y, obs_z, c='gray', s=8, alpha=0.5, label='Obstacles')

        # 初始化无人机标记和尾迹
        colors = plt.get_cmap('tab10')(range(len(uav_ids)))
        uav_scatters = {}  # 无人机实时位置标记
        uav_trails = {}    # 无人机轨迹尾迹
        for idx, uav_id in enumerate(uav_ids):
            # 实时位置（圆形，大尺寸）
            scatter = ax.scatter([], [], [], marker='o', s=150, color=colors[idx], label=f'UAV {uav_id}')
            # 轨迹尾迹（线，透明）
            trail, = ax.plot([], [], [], linewidth=2, color=colors[idx], alpha=0.8)
            uav_scatters[uav_id] = scatter
            uav_trails[uav_id] = trail

        # 设置坐标轴范围（基于所有轨迹的极值）
        all_x = []
        all_y = []
        all_z = []
        for traj in uav_smooth_trajs.values():
            all_x.extend(traj[0])
            all_y.extend(traj[1])
            all_z.extend(traj[2])
        ax.set_xlim(min(all_x) - 5, max(all_x) + 5)
        ax.set_ylim(min(all_y) - 5, max(all_y) + 5)
        ax.set_zlim(min(all_z) - 5, max(all_z) + 5)

        ax.legend(loc='best', fontsize=9)
        ax.view_init(elev=20, azim=45)  # 固定视角

        # 5. 动画更新函数
        def update(frame):
            current_tick = smooth_ticks[frame]
            current_time = current_tick / TIME_SCALE  # 转换为实际时间（秒）
            # 更新每个无人机的位置和尾迹
            for uav_id in uav_ids:
                xs, ys, zs = uav_smooth_trajs[uav_id]
                # 实时位置
                x = xs[frame]
                y = ys[frame]
                z = zs[frame]
                uav_scatters[uav_id]._offsets3d = ([x], [y], [z])
                # 轨迹尾迹（显示最近TRAIL_LENGTH个点）
                start_idx = max(0, frame - TRAIL_LENGTH)
                trail_x = xs[start_idx:frame+1]
                trail_y = ys[start_idx:frame+1]
                trail_z = zs[start_idx:frame+1]
                uav_trails[uav_id].set_data(trail_x, trail_y)
                uav_trails[uav_id].set_3d_properties(trail_z)
            # 更新标题显示时间
            ax.set_title(f'Multi-UAV 3D Animation | Time: {current_time:.2f}s', fontsize=12)
            return list(uav_scatters.values()) + list(uav_trails.values())

        # 6. 生成并播放动画
        print("\nCreating 3D animation (close window to exit)...")
        anim = FuncAnimation(
            fig, update,
            frames=len(smooth_ticks),
            interval=interval,
            blit=False  # 3D图不支持blit=True，需设为False
        )
        plt.tight_layout()
        plt.show()
